#include "ros/ros.h"
#include "jsk_recognition_msgs/BoundingBoxArray.h"
#include "std_msgs/Float64MultiArray.h"
#include "geometry_msgs/Point.h"
#include <cmath>
#include <memory.h>
#define DEBUG 1
#define TO_FILE DEBUG and 0
#if DEBUG
#include <iostream>
#endif
#if TO_FILE
#include <fstream>
#include <streambuf>
#include <stdlib.h>
#include <stdio.h>
#endif
double phi = M_PI_2;
void lidar_callback(const jsk_recognition_msgs::BoundingBoxArray& lidar)noexcept(true)
{
    static int callback_count;
    static std::vector<std::pair<std::pair<double, double>, int>> points;
    if (points.capacity() < 32)
        points.reserve(32);
#if DEBUG
    std::cout << __func__ << callback_count << '\n';
#endif
    if (callback_count < 10)
    {
        ++callback_count;
        for (const auto& p : lidar.boxes)
        {
#if DEBUG
            std::cout << p.pose.position.x << '\t' << p.pose.position.y << '\n';
#endif
            if (p.pose.position.x > -0.5 and p.pose.position.x < 80)// and std::abs(p.pose.position.y) < 5.0)
            {
                bool not_found = true;
                for (auto& each_point: points)
                    if (std::abs(p.pose.position.x -  each_point.first.first) < 0.2 and std::abs(p.pose.position.y - each_point.first.second) < 0.2)
                    //std::pow(p.pose.position.x - each_point.first.first, 2) + std::pow(p.pose.position.y - each_point.first.second, 2) < 2)//0.25)
                    {
                        each_point.first.first = (each_point.first.first * each_point.second + p.pose.position.x) / (each_point.second + 1);
                        each_point.first.second = (each_point.first.second * each_point.second + p.pose.position.y) / (each_point.second + 1);
                        ++each_point.second;
                        not_found = false;
                        break;
                    }
                if (not_found)
                    points.push_back({{p.pose.position.x, p.pose.position.y}, 1});
            }
        }
#if DEBUG
        for (const auto& p : points)
            std::cout << p.first.first << '\t' << p.first.second << '\t' << p.second << '\n';
        std::cout << "--------------------\n";
#endif
        return;
    }
    std::pair<const std::pair<double, double>*, const std::pair<double, double>*> mate[16];
    memset(mate, 0, sizeof(std::pair<std::pair<double, double>*, std::pair<double, double>*>) << 4);
    // p.first.first/second, mate[index].first/second -> first/second
    for (const auto& p : points)
    {
#if DEBUG
        std::cout << p.first.first << '\t' << p.first.second << '\t' << p.second << '\n';
#endif
        if (p.second > 6)// and std::abs(p.pose.position.y) < 5.0)
        {
            double i = (p.first.first - 0.3) / 5.0;
            int index = std::round(i);
            if (std::abs(i - index) < 0.5)//0.2
                    if (mate[index].second)
                    {
                        const double abs_p_y = std::abs(p.first.second);
                        if (abs_p_y < std::abs(mate[index].second->second))
                            if (abs_p_y < std::abs(mate[index].first->second))
                            {
                                mate[index].second = mate[index].first;
                                mate[index].first = &p.first;
                            }
                            else
                                mate[index].second = &p.first;
                    }
                    else if (mate[index].first)
                        if (std::abs(p.first.second) < std::abs(mate[index].first->second))
                        {
                            mate[index].second = mate[index].first;
                            mate[index].first = &p.first;
                        }
                        else
                            mate[index].second = &p.first;
                    else
                        mate[index].first = &p.first;
            }
        }
	int k_count = 0;
	double k = 0;
	for (const auto& p : mate)
		if (p.first and p.second)
		{
			k -= (p.second->first - p.first->first) / (p.second->second - p.first->second);
			++k_count;
#if DEBUG
            std::cout << "检测到的第" << k_count << "对桶的坐标是(" << p.first->first << ", " << p.first->second
                << "), (" << p.second->first << ", " << p.second->second << "), 1 / k = "
                << -(p.second->first - p.first->first) / (p.second->second - p.first->second) << '\n';
#endif
		}
    if (k_count)
	    phi = std::atan(k / k_count);
    if (phi > M_PI /18)
    {
        callback_count = 0;
        points.clear();
    }
#if DEBUG
    static int test_count;
    std::cout << "第" << test_count << "次求初始航向角phi，phi = " << phi * 180 / M_PI << "度\n";
    ++test_count;
#endif
}
bool ready;
void go_callback(const std_msgs::Float64MultiArray& ecu_go)
{
    ready = ecu_go.data[2] > 0.5;//static_cast<int>(ecu_go.data[2]);//防止0的小数表达不精确被判为true
}
int main(int argc, char **argv)noexcept(true)
{
    // std::cout << "phi = ";
    // std::cin >> phi;
    // phi *= M_PI / 180;
    ros::init(argc, argv, "line_path");
    ros::NodeHandle n;
#if TO_FILE
    std::streambuf* cout_buffer(std::cout.rdbuf());
    char path[100];
    realpath("debug.log", path);
    std::cout << path << '\n';
    std::ofstream out(path, std::ios::app);
    std::cout.rdbuf(out.rdbuf());
#endif
    {//限制subscriber的生存期
        // constexpr double phi_limit = M_PI / 18;//10度
#if DEBUG
        std::cout << "检测到角度在±10°或以内时停止检测\n";
#endif
        ros::Subscriber sub(n.subscribe("/detected_bounding_box", 1, lidar_callback));
        while (ros::ok() and std::abs(phi) > M_PI / 18)//do while
            ros::spinOnce();
    }
    // const double p = phi / 2.3;
    constexpr double frequency = 5;//10
    constexpr double radian_to_angle = 180 / M_PI;
    const double tan_phi = std::tan(phi);
    constexpr double double_frequency = frequency + frequency;
    constexpr double delta_v = 0.5;//frequency;
    constexpr double length = 1.511;
    constexpr double double_length = length + length;
    constexpr double v_max = 1.5;
    struct xi
    {
        double x, y, yaw;
        double v, delta;
    }state{0, 0, phi, 0, 0};
    ros::Rate loop_rate(frequency);
    ros::Publisher pub(n.advertise<std_msgs::Float64MultiArray>("/command", 1));
    std_msgs::Float64MultiArray msg;
    msg.data.resize(5);
    msg.data[2] = 1;//不紧急制动
    msg.data[3] = 0;//制动等级，目前没用
    msg.data[4] = 2;//
    msg.data[0] = msg.data[1] = 0;
    for(int i = 0;i < 20;++i)
    {
        pub.publish(msg);
        loop_rate.sleep();
    }
    {
#if DEBUG
        std::cout << "20个速度0，转角0已发完（车辆状态初始化）\n";
#endif
        ros::Subscriber sub(n.subscribe("/ecu_msg", 1, go_callback));
        while (ros::ok() and not ready)
            ros::spinOnce();
#if DEBUG
        std::cout << "go信号已收到\n";
#endif
    }
    while (ros::ok())//std::abs(state.yaw) > 1e-5)
    {
        double v = state.v + delta_v;
        if (v > v_max)
            v = v_max;
        const double sum_v = state.v + v;
        const double s = sum_v / double_frequency;
        const double delta_x = s * std::cos(state.yaw);
        const double x = state.x + delta_x;
        // double tan_delta_devided_by_length = l / std::tan(delta);
        // state.x += tan_delta_devided_by_length * std::sin((state.v + 0.5 * delta_v) / frequency / tan_delta_devided_by_length);
        const double y = tan_phi * (1 - std::exp(-x));
        const double delta_y = y - state.y;
        state.delta = std::atan(double_length * std::sin(std::atan2(delta_y, delta_x) - state.yaw) / std::sqrt(delta_y * delta_y + delta_x * delta_x));// + p;
        const double yaw = state.yaw + s * std::tan(state.delta) / length;
        if (std::abs(yaw) >= std::abs(state.yaw))
            break;
        // state.x += s * std::abs((std::sin(state.yaw) - std::sin(state.yaw - state.delta)) / state.delta);
        state.x = x;
        state.y = y;
        msg.data[0] = state.v = v;
        state.yaw = yaw;
        msg.data[1] = state.delta * radian_to_angle;
#if DEBUG
        std::cout << "y = " << y << ",\tdelta = " << msg.data[1] << ",\tyaw = " << state.yaw << '\n';
#endif
        pub.publish(msg);
        loop_rate.sleep();
    }
#if  DEBUG
    std::cout << "调整完成\n";
#endif
    constexpr double delta_x = v_max / frequency;
    for(msg.data[0] = v_max, msg.data[1] = 0; ros::ok() and state.x < 30; state.x += delta_x)//90
    {
        pub.publish(msg);
        loop_rate.sleep();
    }
#if DEBUG
    std::cout << "75m线\n";
#endif
    for (msg.data[0] = 0; ros::ok() and state.x < 45; state.x += delta_x)//115
    {
        pub.publish(msg);
        loop_rate.sleep();
    }
    // msg.data[4] = 3;//完成
    // pub.publish(msg);
#if DEBUG
    std::cout << "------------完赛--------------\n";
#endif
#if TO_FILE
    std::cout.rdbuf(cout_buffer);
#endif
}
#undef TO_FILE
#undef DEBUG
